#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

#include <cuda_runtime.h>
#include <helper_cuda.h>
#include <helper_functions.h>

#include <log4cxx/logger.h>
#include <base/logging.h>
#include <base/types.cuh>
#include <base/methods.cuh>

#include <gpu/ciratefi_helper.cuh>

namespace recognition {

__host__ __device__ int RadiusToIdx(int radius, CiratefiBoundaries bounds) {
	return (radius - bounds.r.min) / bounds.r.step;
}

__host__ __device__ int IdxToRadius(int idx, CiratefiBoundaries bounds) {
	return bounds.r.min + idx * bounds.r.step;
}

__global__ void _cu_create_C_A__global__(Matrix2d* A, Point2D_lst* circles, _cu_create_C_A__global__KernelArgs* args, Matrix2d* C_A) {
	int x = blockIdx.x * BLOCK_SIZE + threadIdx.x;
	int y = blockIdx.y * BLOCK_SIZE + threadIdx.y;

	CiratefiBoundaries bounds = args->bounds;
	int offset = args->offset;
	int l = args->l;

	int l_i = 0;
	for (int r = bounds.r.min; r <= bounds.r.max; r += bounds.r.step) {
		double res = 0;

		Point2D_lst circle = circles[RadiusToIdx(r, bounds)];
		int npoints = circle.size;
		Point2D* points =  circle.points;

		for (int i = 0; i < npoints; ++i) {
			res += A->elements[(y + points[i].y + offset) * A->width + x + points[i].x + offset];
		}

		C_A->elements[(y * C_A->width + x) * l + l_i] = res / npoints;
		++l_i;
	}
}

__global__ void _cu_create_C_A__shared__(Matrix2d* A, Point2D* circles, SizeAndIdxs* circles_offsets, SizeAndIdxs* circle_blocks,
			_cu_create_C_A__shared__KernelArgs* args, Matrix2d* C_A) {
	extern __shared__ Point2D s[];
	int x = blockIdx.x * BLOCK_SIZE + threadIdx.x;
	int y = blockIdx.y * BLOCK_SIZE + threadIdx.y;
	int thread_idx = threadIdx.y * BLOCK_SIZE + threadIdx.x;

	CiratefiBoundaries bounds = args->bounds;
	int offset = args->offset;
	int l = args->l;
	int num_blocks = args->num_blocks;

	int l_i = 0;
	for (int circle_block_idx = 0; circle_block_idx < num_blocks; ++circle_block_idx) {
		SizeAndIdxs circle_block = circle_blocks[circle_block_idx];

		// Load circle points into shared memory
		for (int i = 0; i < circle_block.size; i += BLOCK_SIZE * BLOCK_SIZE) {
			int smem_i = thread_idx + i;
			if (smem_i < circle_block.size) {
				s[smem_i] = circles[circles_offsets[circle_block.start_idx].start_idx + smem_i];
			}
		}
		__syncthreads();

		for (int r = IdxToRadius(circle_block.start_idx, bounds); r <= IdxToRadius(circle_block.end_idx, bounds); r += bounds.r.step) {
			double res = 0;
			int r_idx = RadiusToIdx(r, bounds);

			int npoints = circles_offsets[r_idx].size;
			Point2D* points =  &s[circles_offsets[r_idx].start_idx - circles_offsets[circle_block.start_idx].start_idx];

			for (int i = 0; i < npoints; ++i) {
				res += A->elements[(y + points[i].y + offset) * A->width + x + points[i].x + offset];
			}

			C_A->elements[(y * C_A->width + x) * l + l_i] = res / npoints;
			++l_i;
		}
	}
}

__host__ __device__ double corr(double* x, double* y, int npoints, BCThresholds bc_thresholds) {
	double res = 0;

	double x_mean, y_mean;
	x_mean = y_mean = 0;
	for (int i = 0; i < npoints; ++i) {
		x_mean += x[i];
		y_mean += y[i];
	}

	x_mean /= npoints;
	y_mean /= npoints;

	double sum_x_centered_mul_y_centered = 0;
	double sum_x_centered_squared = 0;
	double sum_y_centered_squared = 0;

	for (int i = 0; i < npoints; ++i) {
		double xi_centered, yi_centered;
		xi_centered = x[i] - x_mean;
		yi_centered = y[i] - y_mean;

		sum_x_centered_mul_y_centered += xi_centered * yi_centered;
		sum_x_centered_squared += xi_centered * xi_centered;
		sum_y_centered_squared += yi_centered * yi_centered;
	}

	double beta = sum_x_centered_squared == 0 ? 0 : sum_x_centered_mul_y_centered / sum_x_centered_squared;
	double gamma = y_mean - beta * x_mean;

	double divisor = sqrt(sum_x_centered_squared) * sqrt(sum_y_centered_squared);
	res = divisor == 0 ? 0 : sum_x_centered_mul_y_centered / divisor;

	// Check for thresholds of contrast and brightness
	res = (abs(beta) <= bc_thresholds.beta || 1 / bc_thresholds.beta <= abs(beta)
				|| abs(gamma) > bc_thresholds.gamma) ? 0 : res;

	return res;
}

__global__ void _cu_CisCorr_AQ__global__(Matrix2d* CQ, Matrix2d* CA, _cu_CisCorr_AQ__global__KernelArgs* args, FirstGradeMatrix* CisCorr_mat) {
	int x = blockIdx.x * BLOCK_SIZE + threadIdx.x;
	int y = blockIdx.y * BLOCK_SIZE + threadIdx.y;

	CiratefiThresholds thresholds = args->thresholds;
	BCThresholds bc_thresholds = args->bc_thresholds;
	int l = args->l;
	int n = args->n;

	int scale_idx = -1;
	double max_corr = 0;

	double* CAxy = &(CA->elements[(y * CA->width + x) * l]);
	for (int i = 0; i < n; ++i) {
		double* CQi = &(CQ->elements[i * l]);
		double corr_ = abs(corr(CQi, CAxy, l, bc_thresholds));
		if (corr_ > thresholds.cifi_t1 && corr_ > max_corr) {
			max_corr = corr_;
			scale_idx = i;
		}
	}

	CisCorr_mat->scale_idxs[y * CisCorr_mat->width + x] = scale_idx;
}

__global__ void _cu_CisCorr_AQ__shared__(Matrix2d* CQ, Matrix2d* CA, _cu_CisCorr_AQ__global__KernelArgs* args, FirstGradeMatrix* CisCorr_mat) {
	extern __shared__ double s_CisCorr_AQ[];
	int x = blockIdx.x * BLOCK_SIZE + threadIdx.x;
	int y = blockIdx.y * BLOCK_SIZE + threadIdx.y;
	int thread_idx = threadIdx.y * BLOCK_SIZE + threadIdx.x;

	CiratefiThresholds thresholds = args->thresholds;
	BCThresholds bc_thresholds = args->bc_thresholds;
	int l = args->l;
	int n = args->n;

	int scale_idx = -1;
	double max_corr = 0;

	// Load CQ into shared memory
	int CQ_size = CQ->width * CQ->height;
	for (int i = 0; i < CQ_size; i += BLOCK_SIZE * BLOCK_SIZE) {
		int smem_i = thread_idx + i;
		if (smem_i < CQ_size) {
			s_CisCorr_AQ[smem_i] = CQ->elements[smem_i];
		}
	}
	__syncthreads();

	double* CAxy = &(CA->elements[(y * CA->width + x) * l]);
	for (int i = 0; i < n; ++i) {
		double* CQi = &(s_CisCorr_AQ[i * l]);
		double corr_ = abs(corr(CQi, CAxy, l, bc_thresholds));
		if (corr_ > thresholds.cifi_t1 && corr_ > max_corr) {
			max_corr = corr_;
			scale_idx = i;
		}
	}

	CisCorr_mat->scale_idxs[y * CisCorr_mat->width + x] = scale_idx;
}

__host__ __device__ int AngleToIdx(double angle, CiratefiBoundaries bounds) {
	// FIXIT: adding 0.000001 to eliminate double precision difference on CPU and GPU
	return (angle - bounds.phi.min) / bounds.phi.step + 0.000001;
}

__host__ __device__ double IdxToAngle(int idx, CiratefiBoundaries bounds) {
	return bounds.phi.min + idx * bounds.phi.step;
}

__global__ void _cu_create_R_A__global__(Matrix2d* A, FirstGradePoint* fgps, int fgps_size, Line_lst* lines,
		_cu_create_R_A__global__KernelArgs *args, double* R_A) {
	int tid = blockIdx.x * blockDim.x + threadIdx.x;

	if (tid >= fgps_size)
			return;

	CiratefiBoundaries bounds = args->bounds;
	int offset = args->offset;
	int m = args->m;

	FirstGradePoint fgp = fgps[tid];

	int x, y;
	double s;
	x = fgp.p.x;
	y = fgp.p.y;
	s = bounds.s.min + fgp.scale_idx * bounds.s.step;
	int lambda = s * bounds.r.max;

	Line_lst line_lst = lines[lambda];

	for (int j = 0; j < m; ++j) {
		Point2D* points = &(line_lst.points[line_lst.size_and_idxs[j].start_idx]);
		int npoints = line_lst.size_and_idxs[j].size;

		double res = 0;
		for (int i = 0; i < npoints; ++i) {
			res += A->elements[(y + points[i].y + offset) * A->width + x + points[i].x + offset];
		}

		R_A[tid * m + j] = res / npoints;
	}
}

__global__ void _cu_RasCorr_AQ__global__(DArrayWrap* RQjs, double* R_A, int fgps_size,
		_cu_RasCorr_AQ__global__KernelArgs *args, int* angle_idxs) {
	int tid = blockIdx.x * blockDim.x + threadIdx.x;

	if (tid >= fgps_size)
		return;

	BCThresholds bc_thresholds = args->bc_thresholds;
	CiratefiThresholds thresholds = args->thresholds;
	int m = args->m;

	int angle_idx = -1;
	double max_corr = 0;

	double* RAxy = &(R_A[tid * m]);
	for (int j = 0; j < m; ++j) {
		double corr_ = abs(corr(RQjs[j].elements, RAxy, m, bc_thresholds));
		if (corr_ > thresholds.rafi_t2 && corr_ > max_corr) {
			max_corr = corr_;
			angle_idx = j;
		}
	}

	angle_idxs[tid] = angle_idx;
}

__host__ __device__ int ScaleToIdx(double scale, CiratefiBoundaries bounds) {
	// FIXIT: adding 0.000001 to eliminate double precision difference on CPU and GPU
	return (scale - bounds.s.min) / bounds.s.step + 0.000001;
}

__host__ __device__ double IdxToScale(int idx, CiratefiBoundaries bounds) {
	return bounds.s.min + idx * bounds.s.step;
}

__global__ void _cu_eval_Tefi__global__(double* Q_centered, Matrix2d* A, SecondGradePoint* sgps, int sgps_size,
		_cu_eval_Tefi__global__KernelArgs *args, double* correlations) {
	int tid = blockIdx.x * blockDim.x + threadIdx.x;

	if (tid >= sgps_size)
		return;

	correlations[tid] = 0;

	BCThresholds bc_thresholds = args->bc_thresholds;
	CiratefiThresholds thresholds = args->thresholds;
	CiratefiBoundaries bounds = args->bounds;
	int offset = args->offset;
	int w1 = args->w1;
	int h1 = args->h1;
	double Q_mean = args->Q_mean;
	double sum_Q_centered_squared = args->sum_Q_centered_squared;

	int half_w1, half_h1;
	half_w1 = w1 / 2;
	half_h1 = h1 / 2;

	SecondGradePoint* sgp = &(sgps[tid]);

	double max_corr = 0;
	double s_best = bounds.s.min + sgp->scale_idx * bounds.s.step;
	double phi_best = bounds.phi.min + sgp->angle_idx * bounds.phi.step;
	for (double s = s_best - bounds.s.step; s <= s_best + bounds.s.step; s += bounds.s.step) {
		for (double phi = phi_best - bounds.phi.step; phi <= phi_best + bounds.phi.step; phi += bounds.phi.step) {
			double a11, a12, a21, a22, a13, a23;
			// -phi Counterclockwise
			a11 = s * cos(-phi); a12 = -s * sin(-phi);
			a21 = s * sin(-phi); a22 = s * cos(-phi);
			a13 = sgp->p.x + offset; a23 = sgp->p.y + offset;

			double A_mean = 0;
			for (int y = -half_h1; y < h1 - half_h1; ++y) {
				for (int x = -half_w1; x < w1 - half_w1; ++x) {
					int target_x, target_y;
					target_x = a11 * x + a12 * y + a13;
					target_y = a21 * x + a22 * y + a23;

					A_mean += A->elements[target_y * A->width + target_x];
				}
			}
			A_mean /= (h1 * w1);

			double sum_Q_centered_mul_A_centered = 0;
			double sum_A_centered_squared = 0;
			for (int y = -half_h1; y < h1 - half_h1; ++y) {
				for (int x = -half_w1; x < w1 - half_w1; ++x) {
					int target_x, target_y;
					target_x = a11 * x + a12 * y + a13;
					target_y = a21 * x + a22 * y + a23;

					double A_centered_val, Q_centered_val;
					A_centered_val = A->elements[target_y * A->width + target_x] - A_mean;
					Q_centered_val = Q_centered[(y + half_h1) * w1 + x + half_w1];

					sum_Q_centered_mul_A_centered += Q_centered_val * A_centered_val;
					sum_A_centered_squared += A_centered_val * A_centered_val;
				}
			}

			double beta = sum_Q_centered_squared == 0 ? 0 : sum_Q_centered_mul_A_centered / sum_Q_centered_squared;
			double gamma = A_mean - beta * Q_mean;

			double divisor = sqrt(sum_Q_centered_squared) * sqrt(sum_A_centered_squared);
			double corr = divisor == 0 ? 0 : sum_Q_centered_mul_A_centered / divisor;

			// Check for thresholds of contrast and brightness
			corr = (abs(beta) <= bc_thresholds.beta || 1 / bc_thresholds.beta <= abs(beta)
						|| abs(gamma) > bc_thresholds.gamma) ? 0 : abs(corr);

			if (corr > max_corr) {
				max_corr = corr;
				sgp->scale_idx = ScaleToIdx(s, bounds);
				sgp->angle_idx = AngleToIdx(phi, bounds);

			}
		}
	}

	correlations[tid] = max_corr;
}

void CirclePoints(int x, int y, std::vector<Point2D>& circle) {
	circle.push_back(Point2D(x, y));

	if (x == 0 && y == 0)
		return;

	circle.push_back(Point2D(-x, y));
	circle.push_back(Point2D(x, -y));
	circle.push_back(Point2D(-x, -y));

	if (x != y) {
		circle.push_back(Point2D(y, x));
		circle.push_back(Point2D(y, -x));
		circle.push_back(Point2D(-y, x));
		circle.push_back(Point2D(-y, -x));
	}
}

std::vector<Point2D> MidpointCircle(int radius, int angle_incr) {
	angle_incr = angle_incr < 1 ? 1 : angle_incr;
	int x = 0, y = radius;
	int step = 0;
	int d = 1 - radius, deltaE = 3, deltaSE = 5 - 2 * radius;
	std::vector<Point2D> circle;
	CirclePoints(x, y, circle);

	while (y > x) {
		if (d < 0) {		/* select E */
			d += deltaE;
			deltaE += 2;
			deltaSE += 2;
		} else {
			d += deltaSE;	/* select SE */
			deltaE += 2;
			deltaSE += 4;
			--y;
		}
		++x;

		if ((++step % angle_incr) == 0) {
			CirclePoints(x, y, circle);
		}
	}

	return circle;
}

std::vector<Point2D> MidpointLine(int x0, int y0, int x1, int y1, int line_incr) {
	int dx, dy, step = 0, d, incrE, incrNE, x, y;
	Point2D p;
	bool swap = false, x_swapped = false, y_swapped = false;
	std::vector<Point2D> line;

	// Swap coordinates
	if (x0 > x1 && y0 > y1) {
		SWAP_INTS(x0, x1);
		SWAP_INTS(y0, y1);
	} else if (x0 > x1) {
		x1 = -x1;
		x0 = -x0;
		x_swapped = true;
	} else if (y0 > y1) {
		y1 = -y1;
		y0 = -y0;
		y_swapped = true;
	}

	dx = x1 - x0;
	dy = y1 - y0;

	// Swap coordinates, if slope > 1
	if (dx < dy) {
		x1 -= x0;
		y1 -= y0;

		SWAP_INTS(x1, y1);

		x1 += x0;
		y1 += y0;

		dx = x1 - x0;
		dy = y1 - y0;

		swap = true;
	}

	d = 2 * dy - dx;		/* initial value of d */
	incrE = 2 * dy;			/* increment used for move to E */
	incrNE = 2 * (dy - dx); /* increment used for move to NE */
	x = x0;
	y = y0;

	p.x = x;
	p.y = y;

	if (x_swapped) {
		p.x = -p.x;
	} else if (y_swapped) {
		p.y = -p.y;
	}
	line.push_back(p);

	while (x < x1) {
		if (d <= 0) {		/* choose E */
			d += incrE;
			x++;
		} else {			/* choose NE */
			d += incrNE;
			x++;
			y++;
		}

		if (swap) {
			p.x = x - x0;
			p.y = y - y0;

			SWAP_INTS(p.x, p.y);

			p.x += x0;
			p.y += y0;
		} else {
			p.x = x;
			p.y = y;
		}

		if (x_swapped) {
			p.x = -p.x;
		} else if (y_swapped) {
			p.y = -p.y;
		}

		// Skip
		if ((++step % line_incr) == 0) {
			line.push_back(p);
		}
	}

	return line;
}

void ReportCiratefiResult(log4cxx::LoggerPtr logger, CiratefiBoundaries bounds, CiratefiResult res, double max_corr) {
	double best_scale = IdxToScale(res.best_match.scale_idx, bounds);
	double best_angle = IdxToAngle(res.best_match.angle_idx, bounds);
	LOG_INFO(logger, "Maximum correlation: %.5f in (x, y)=(%d, %d) with [s: %.2f (%%%d), phi: %.4f rad (%d deg)]", max_corr,
			res.best_match.p.x, res.best_match.p.y,
			best_scale, (int) (best_scale * 100),
			best_angle, (int) (180 * best_angle / M_PI) );
}

}	// namespace recognition
